#pragma once
#include "stm32f1xx_hal.h"
#include "PIDProvider.h"
#include "MotorController.h"

#define LOG_TAG "PathController"

#include <elog.h>

/**
 * @brief 码盘转动一格, 前进的长度(cm)
 */
#define CODE_WHEEL_ONE_STEP ((float)1.05243353895)

/**
 * @brief 走廊长度(cm)
 */
#define CORRIDOR_DISTANCE_ROAD ((float)60)
#define CORRIDOR_DISTANCE_ROOM ((float)50)

/**
 * @brief 转过90度经过的距离
 */
#define DISTANCE_ROTATE_90 ((float)20.4203522483)

/**
 * @brief 转动时车辆速度
 */
#define CAR_SPEED_ROTATE 4

/**
 * @brief 直行时车辆速度
 */
#define CAR_SPEED_FORWARD 4

#define MAX_ROOM 8
#define MAX_NODE (MAX_ROOM + 6)

#define ROOM(x) (x + 5)

#define MAX_CAR_SPEED 8

typedef uint8_t Direction;

#define DIR_LEFT ((Direction)0)
#define DIR_RIGHT ((Direction)1)
#define DIR_UP ((Direction)2)
#define DIR_DOWN ((Direction)3)

typedef struct Node
{
    struct Node *prev;
    struct Node *next;

    uint8_t id;
    /**
     * @brief 从上一点指向此点方向
     */
    Direction direction;
    /**
     * @brief 是否是病房
     */
    uint8_t isRoom;
    /**
     * @brief 需要识别房间编号个数
     */
    uint8_t labelNum;
} Node_t, *pNode;

/**
 * @brief Path PID参数
 */
struct PIDParam Path_PIDParam = {
    .P = -10,
    .D = 64};

/**
 * @brief 节点数组, [0]为起始节点
 */
Node_t GuideNodes[MAX_NODE];

/**
 * @brief 房间映射
 */
pNode RoomMap[MAX_ROOM];

/**
 * @brief 前进计数
 */
uint32_t Path_L;
uint32_t Path_R;

/**
 * @brief 当前前进了的距离
 */
float curDistance;

/**
 * @brief 预期前进的距离
 */
float expectDistance;

/**
 * @brief 当前机身朝向
 */
Direction curDir;

/**
 * @brief 当前处在的节点
 */
pNode curNode;

/**
 * @brief 目标节点
 */
pNode targetNode;

/**
 * @brief 车辆前进速度
 */
float CarSpeed;

/**
 * @brief 位置环
 */
PIDProvider_t Path_PID;

/**
 * @brief 根据当前节点及前往的地点确定路程
 * 
 * @param curNode 当前节点
 * @param targetNode 目标节点
 */
float get_path_distance(pNode curNode, pNode targetNode)
{
    //是否前往房间
    if (targetNode->isRoom)
    {
        if (curNode->next->isRoom)
            return CORRIDOR_DISTANCE_ROOM;
        else
            return CORRIDOR_DISTANCE_ROAD;
    }
    //从房间离开
    else
    {
        if (curNode->isRoom)
            return CORRIDOR_DISTANCE_ROOM;
        else
            return CORRIDOR_DISTANCE_ROAD;
    }
}

/**
 * @brief 根据当前节点及前往的地点确定机身朝向
 * 
 * @param curNode 当前节点
 * @param targetNode 目标节点
 * @return Direction 
 */
Direction get_next_direction(pNode curNode, pNode targetNode)
{
    //是否前往房间
    if (targetNode->isRoom)
    {
        return curNode->next->direction;
    }
    //从房间离开
    else
    {
        switch (curNode->next->direction)
        {
        case DIR_UP:
            return DIR_DOWN;
        case DIR_DOWN:
            return DIR_UP;
        case DIR_LEFT:
            return DIR_RIGHT;
        case DIR_RIGHT:
            return DIR_LEFT;
        default:
            break;
        }
    }
}

/**
 * @brief 根据识别标签时所处朝向, 计算相对朝向的真实朝向
 * 
 * @param identify 识别时朝向
 * @param relative 相对朝向
 * @return Direction 真实朝向
 */
Direction get_actual_direction(Direction identify, Direction relative)
{
    switch (identify)
    {
    case DIR_UP:
        switch (relative)
        {
        case DIR_LEFT:
            return DIR_LEFT;
        case DIR_RIGHT:
            return DIR_RIGHT;
        }
    case DIR_DOWN:
        switch (relative)
        {
        case DIR_LEFT:
            return DIR_RIGHT;
        case DIR_RIGHT:
            return DIR_LEFT;
        }
    case DIR_LEFT:
        switch (relative)
        {
        case DIR_LEFT:
            return DIR_DOWN;
        case DIR_RIGHT:
            return DIR_UP;
        }
    case DIR_RIGHT:
        switch (relative)
        {
        case DIR_LEFT:
            return DIR_UP;
        case DIR_RIGHT:
            return DIR_DOWN;
        }
    }
}

/**
 * @brief 根据当前方向和目标方向计算旋转角度
 * 
 * @param curDir 当前方向
 * @param targetDir 目标方向
 * @return 旋转角度, 正逆时针转,负顺时针转
 */
uint32_t get_rotate_degree(Direction curDir, Direction targetDir)
{
    switch (curDir)
    {
    case DIR_UP:
        switch (targetDir)
        {
        case DIR_UP:
            return 0;
        case DIR_DOWN:
            return 180;
        case DIR_LEFT:
            return 90;
        case DIR_RIGHT:
            return -90;
        }
    case DIR_DOWN:
        switch (targetDir)
        {
        case DIR_UP:
            return 180;
        case DIR_DOWN:
            return 0;
        case DIR_LEFT:
            return -90;
        case DIR_RIGHT:
            return 90;
        }
    case DIR_LEFT:
        switch (targetDir)
        {
        case DIR_UP:
            return -90;
        case DIR_DOWN:
            return 90;
        case DIR_LEFT:
            return 0;
        case DIR_RIGHT:
            return 180;
        }
    case DIR_RIGHT:
        switch (targetDir)
        {
        case DIR_UP:
            return 90;
        case DIR_DOWN:
            return -90;
        case DIR_LEFT:
            return 180;
        case DIR_RIGHT:
            return 0;
        }
    }
}

/**
 * @brief 走一段距离
 * 
 * @param speedL 左侧速度
 * @param speedR 右侧速度
 * @param distance 距离
 */
void walk_distance(float speedL, float speedR, float distance)
{
    log_d("Walk:L:%f, R:%f, Distance:%f", speedL, speedR, distance);
    Speed_L = speedL;
    Speed_R = -speedR;

    curDistance = 0;

    while (curDistance < distance)
    {
        curDistance += (float)(Path_L + Path_R) / 2 * CODE_WHEEL_ONE_STEP;
        Path_L = Path_R = 0;

        update_code_wheel_speed();
        HAL_Delay(CODE_WHEEL_UPDATE_INTERVAL * 1000);
    }

    Speed_L = Speed_R = 0;
    update_code_wheel_speed();

    curDistance = 0;
}

/**
 * @brief 旋转指定角度
 * 
 * @param degree 
 */
void rotate_degree(uint32_t degree)
{
    if (degree == 0)
        return;

    log_d("Rotate:%d", degree);

    float distance = DISTANCE_ROTATE_90 * (abs(degree) / 90);

    //逆时针转
    if (degree > 0)
    {
        walk_distance(-CAR_SPEED_ROTATE, CAR_SPEED_ROTATE, distance);
    }
    //顺时针转
    else
    {
        walk_distance(CAR_SPEED_ROTATE, -CAR_SPEED_ROTATE, distance);
    }
}

/**
 * @brief 根据识别信息更新路径信息
 * 
 * @param roomId 房间编号
 * @param identifyNodeId 识别时位于的节点
 * @param dir 从上一节点指向房间的方向
 */
void onIdentifyLabel(uint8_t roomId, uint8_t identifyNodeId, Direction dir)
{
    Node_t identifyNode = GuideNodes[identifyNodeId];
    Node_t roomNode = GuideNodes[ROOM(roomId)];

    roomNode.direction = dir;
    roomNode.prev = &identifyNode;
}

/**
 * @brief 使用OpenMV检测房间编号
 * 
 * @param identifyNode 进行识别的节点
 * @param targetNode 目标节点
 */
void update_room_label(pNode identifyNode, pNode targetNode)
{
    uint8_t labelNum = identifyNode->labelNum;
    log_i("Identify Node:%d, labelNum:%d", identifyNode->id, labelNum);


}

/**
 * @brief 根据距离目标点距离确定车辆前进速度
 */
void update_path()
{
    if (curNode == targetNode)
        return;

    curDistance += (float)(Path_L + Path_R) / 2 * CODE_WHEEL_ONE_STEP;
    Path_L = Path_R = 0;

    float deltaDistance = expectDistance - curDistance;

    log_i("CurDis:%f, deltaDis:%f", curDistance, deltaDistance);

    //到达下一节点
    if (deltaDistance < 0)
    {
        CarSpeed = 0;
        curDistance = 0;

        pNode nextNode = curNode->next;
        //到达目标点
        if (nextNode == targetNode)
        {
            log_i("Arrive Target:%d", nextNode->id);
            //触发回调
            curNode = nextNode;
            return;
        }
        else
        {
            //检测房间编号信息
            update_room_label(nextNode, targetNode);

            //旋转车辆至下一朝向
            Direction nextDir = get_next_direction(curNode, targetNode);
            uint32_t degree = get_rotate_degree(curDir, nextDir);
            rotate_degree(degree);

            //计算下一路径距离
            expectDistance = get_path_distance(curNode, targetNode);

            curNode = nextNode;
        }
    }

    //刷新车辆速度
    CarSpeed += pid_push_new_err(&Path_PID, deltaDistance);
    min(CarSpeed, MAX_CAR_SPEED);
}

/**
 * @brief 根据目标节点构建地图
 * 
 * @param startNode 起始节点
 * @param targetNode 目标节点
 */
void build_path(pNode startNode, pNode targetNode)
{
    log_i("Start build path:");
    //从起始节点到房间
    if (targetNode->isRoom)
    {
        log_i("Start->Room");
        //路径已存在
        if (targetNode->prev)
        {
            log_i("Path Exists:");
            pNode curNode = targetNode;
            //反向搜索路径
            while (curNode->prev)
            {
                log_i("Path: %d <- %d", curNode->id, curNode->prev->id);
                curNode->prev->next = curNode;
                curNode = curNode->prev;
            }
            log_i("Path End!");
        }
        else
        {
            //指向中部节点
            GuideNodes[1].next = &GuideNodes[2];
            //清空十字路口指向目标
            GuideNodes[2].next = NULL;
            GuideNodes[3].next = NULL;
            GuideNodes[4].next = NULL;
            GuideNodes[5].next = NULL;
        }
    }
    else
    {
        log_i("Room->Start");
        pNode curNode = startNode;
        //反向搜索路径
        while (curNode->prev)
        {
            log_i("Path: %d -> %d", curNode->id, curNode->prev->id);
            curNode->next = curNode->prev;
            curNode = curNode->prev;
        }
        log_i("Path End!");
    }
}

/**
 * @brief 初始化节点
 */
void setup_nodes()
{
    //起始节点
    GuideNodes[0].next = &GuideNodes[1];
    //中部节点
    GuideNodes[2].labelNum = 2;
    GuideNodes[2].prev = &GuideNodes[1];
    //远端节点
    GuideNodes[3].labelNum = 4;
    GuideNodes[3].prev = &GuideNodes[2];
    //远端左
    GuideNodes[4].direction = DIR_LEFT;
    GuideNodes[4].labelNum = 2;
    GuideNodes[4].prev = &GuideNodes[3];
    //远端右
    GuideNodes[5].direction = DIR_RIGHT;
    GuideNodes[5].labelNum = 2;
    GuideNodes[5].prev = &GuideNodes[3];
    //Room1
    GuideNodes[ROOM(1)].prev = &GuideNodes[1];
    GuideNodes[ROOM(1)].direction = DIR_LEFT;
    //Room2
    GuideNodes[ROOM(2)].prev = &GuideNodes[1];
    GuideNodes[ROOM(2)].direction = DIR_RIGHT;

    for (size_t i = ROOM(1); i < MAX_NODE; i++)
    {
        GuideNodes[i].isRoom = true;
    }

    for (size_t i = 0; i < MAX_NODE; i++)
    {
        GuideNodes[i].id = i;
    }
}